#include "startup.h"
#include "main.h"
#include "../Core/led/led.h"
#include "../MT6701CT_STD/MT6701CT_STD.h"
#include "../motor/Motor.h"
#include "../pid/PidCotroler.h"
#include "../Inc/usart.h"
extern TIM_HandleTypeDef htim1;
extern MT6701SensorData sensorData;
PidController speedPid(0.2f, 0.5f, 0.2f);


void startup() {
    Led &led = Led::instance();
    HAL_TIM_Base_Start_IT(&htim1);  // 启动更新中断
	// 或
	HAL_TIM_OC_Start_IT(&htim1, TIM_CHANNEL_1);  // 启动输出比较中断（通道1）
	HAL_TIM_OC_Start_IT(&htim1, TIM_CHANNEL_2);  // 启动输出比较中断（通道1）
	HAL_TIM_OC_Start_IT(&htim1, TIM_CHANNEL_3);  // 启动输出比较中断（通道1）
    // 设置电机初始速度
    //BLDCMotor::getInstance().setSpeed(2000);
    // 启动电机
    // 初始化电机和传感器
	// 这里只初始化一次，不需要做任何事了
	BLDCMotor::getInstance().init();
	BLDCMotor::getInstance().start();
	MT6701Sensor::getInstance()->alignSensor();
	BLDCMotor::getInstance().openLoopStart();
	speedPid.setOutputLimits(0.0f, 1000.0f);

	/* 启动串口接收中断 */
	//HAL_UART_Receive_IT(&huart2, &rxData[0], 1);
    // 主循环
    while (1) {
		// for (int i=1;i<=6;i++) {
		// 	BLDCMotor::getInstance().executeCommutation(static_cast<CommutationStep>(i));
		// 	HAL_Delay(1);
		// }
    }
}
